#include "QEPDriver.h"   // Example specific Include file
#include "..\System\F2806x_Device.h"

void QEPDriver::Init(int encoPPR, int polePairs, int calibAngle)
{
	EQep1Regs.QDECCTL.bit.QSRC = 0;	// QEP quadrature count mode
 	EQep1Regs.QDECCTL.bit.XCR = 0; 	// 2x resolution

 	EQep1Regs.QEPCTL.bit.FREE_SOFT=2; 	// QEP Emulation Free
	EQep1Regs.QEPCTL.bit.PCRM = 0;		// PCRM=00 - Position counter reset on an index event
	EQep1Regs.QEPCTL.bit.IEI = 2; 		// IEI=10 - Initializes the position counter on the rising edge of the QEPI signal
	EQep1Regs.QEPCTL.bit.IEL = 1;		// IEL=01 - Latches position counter on rising edge of the index signal
	EQep1Regs.QEPCTL.bit.QCLM=1;        // Latch on unit time out
	EQep1Regs.QEPCTL.bit.UTE=1;         // Unit Timeout Enable
	EQep1Regs.QEPCTL.bit.QPEN=1;        // QEP enable

	EQep1Regs.QPOSCTL.all = 0; 		// Disable position compare unit
	EQep1Regs.QUPRD=80000;         	// Unit Timer for 1000Hz at 80 MHz SYSCLKOUT

	EQep1Regs.QCAPCTL.bit.UPPS=1;       // 1/2 for unit position // XXX
	EQep1Regs.QCAPCTL.bit.CCPS=7;       // 1/128 for CAP clock (SYSCLKOUT/128) //  XXX
	EQep1Regs.QCAPCTL.bit.CEN=1;        // QEP Capture Enable
	
	EQep1Regs.QPOSMAX = 4*encoPPR; // total pulses per revolution (4x)

	// set pins
	EALLOW;                       // Enable EALLOW
	GpioCtrlRegs.GPAMUX2.bit.GPIO20 = 1;  // GPIO20 is EQEP1A
	GpioCtrlRegs.GPAMUX2.bit.GPIO21 = 1;  // GPIO21 is EQEP1B
	GpioCtrlRegs.GPAMUX2.bit.GPIO22 = 1;  // GPIO22 is EQEP1S
	GpioCtrlRegs.GPAMUX2.bit.GPIO23 = 1;  // GPIO23 is EQEP1I
	EDIS;                         // Disable EALLOW

 	// reset values
	m_IndexLocked = false;
 	m_currentPosition = 0;
 	m_ElecTheta = 0;
 	m_MechTheta = 0;
 	m_lastLatchedPos = 0;
 	m_Direction = 0;
 	m_SpeedFast = 0;
 	m_SpeedSlow = 0;
 	
 	m_PolePairs = polePairs;
	m_EncoPPR = encoPPR;
	m_CalibratedAngle = calibAngle;

 	// convert to rad/s
	m_ToRadPerSec = 1.0f/(m_EncoPPR*4) * 6.283185f; // m_EncoPPR*4 pulses per revolution
 	m_ToRPM = 1.0f/(m_EncoPPR*4) * 60; // to RPM

 	m_MechScaler = 0.25/m_EncoPPR; // 4*slits-> 4*500=2000
}

float QEPDriver::GetSpeedPulsesPerSec(void)
{
	float speed=0;
	
	if(fabs(m_SpeedFast) > 2000 ) // 2000 pulses per second
	{
		speed = m_SpeedFast;
	}
	else
	{
		speed = m_SpeedSlow;
	}
	
	return speed;
}

float QEPDriver::GetSpeedRads()
{
	return GetSpeedPulsesPerSec()*m_ToRadPerSec;
}

float QEPDriver::GetSpeedRPM()
{
	return GetSpeedPulsesPerSec()*m_ToRPM;
}

float QEPDriver::GetPosition(void)
{
	return (float)m_currentPosition; // return last QPOSCNT
}

void QEPDriver::Calc(void)
{
	// get current position
	m_currentPosition = EQep1Regs.QPOSCNT;
	m_Direction = EQep1Regs.QEPSTS.bit.QDF;

	/* Check the position counter for EQEP1 */
	int32 RawTheta = m_currentPosition + m_CalibratedAngle;
	if (RawTheta < 0)
	{
		RawTheta = RawTheta + EQep1Regs.QPOSMAX;
	}
	else if (RawTheta > EQep1Regs.QPOSMAX)
	{
		RawTheta = RawTheta - EQep1Regs.QPOSMAX;
	}

	/* Compute the mechanical angle */
	m_MechTheta = m_MechScaler*RawTheta;

	/* Compute the electrical angle */
	m_ElecTheta = (m_PolePairs*m_MechTheta) - floor(m_PolePairs*m_MechTheta);

	/* Check an index occurrence*/
	if (EQep1Regs.QFLG.bit.IEL == 1)
	{
		m_IndexLocked = true;
		m_QepCountIndex = EQep1Regs.QPOSILAT;
		EQep1Regs.QCLR.bit.IEL = 1;	/* Clear interrupt flag */
	}
     
	//**** High Speed Calcultation using QEP Position counter ****//
	// Unit Timer is configured for 1000Hz in INIT function
    if(EQep1Regs.QFLG.bit.UTO==1)                    // If unit timeout (one 1000Hz period)
    {
        /** Differentiator  **/
        Uint32 currentLatchedPos = EQep1Regs.QPOSLAT;                // Latched POSCNT value
        Int32 countsPerRev = m_EncoPPR*4;
        Int32 diff = (Int32)currentLatchedPos - (Int32)m_lastLatchedPos;
        if( diff > countsPerRev/2) diff -= countsPerRev;
        else if( diff < -countsPerRev/2) diff += countsPerRev;

        m_SpeedFast = diff * 1000; // pulses per second
        
        m_lastLatchedPos = currentLatchedPos; // remember old state
        EQep1Regs.QCLR.bit.UTO=1;                   // Clear interrupt flag
    }
     
    //**** Low-speed computation using QEP capture counter ****//
    if(EQep1Regs.QEPSTS.bit.UPEVNT==1)                 // Unit position event
    {
    	Uint32 period = EQep1Regs.QCPRDLAT;
    	
    	float newSpeedSlow = 0;
    	if( (EQep1Regs.QEPSTS.bit.COEF==0) && (period != 0))
    	{
    		newSpeedSlow = 2*625000.0f/period; // 2 pulses per period
    	}
    	else
    	{
    		// overflow, set speed to ZERO
    		newSpeedSlow = 0;
    	}
    	
    	if( m_Direction==0 )
    	{
    		newSpeedSlow = -newSpeedSlow; // CCW, invert speed
    	}
      
      	m_SpeedSlow = newSpeedSlow;
      
        EQep1Regs.QEPSTS.all=0x88;                  // Clear Unit position event flag
                                                    // Clear overflow error flag
    }
     
    // check slow timer overflow (if motor is stopped!)
	if( EQep1Regs.QEPSTS.bit.COEF==1)
	{
		m_SpeedSlow = 0;
	}
}

bool QEPDriver::IsIndexLocked(void)
{
	return m_IndexLocked;
}

float QEPDriver::GetMechTheta(void)
{
	return m_MechTheta;
}

float QEPDriver::GetElecTheta(void)
{
	return m_ElecTheta;
}

void QEPDriver::SetCalibAngle(int32 calibAngle)
{
	m_CalibratedAngle = calibAngle;
}
